/**
 * @author Russell Toris - rctoris@wpi.edu
 */

var ROS2D = ROS2D || {
  REVISION: '0.8.0-SNAPSHOT'
};

// convert the given global Stage coordinates to ROS coordinates
createjs.Stage.prototype.globalToRos = function (x, y) {
  var rosX = (x - this.x) / this.scaleX;
  var rosY = (this.y - y) / this.scaleY;
  return new ROSLIB.Vector3({
    x: rosX,
    y: rosY
  });
};

// convert the given ROS coordinates to global Stage coordinates
createjs.Stage.prototype.rosToGlobal = function (pos) {
  var x = pos.x * this.scaleX + this.x;
  var y = pos.y * this.scaleY + this.y;
  return {
    x: x,
    y: y
  };
};

// convert a ROS quaternion to theta in degrees
createjs.Stage.prototype.rosQuaternionToGlobalTheta = function (orientation) {
  // See https://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles#Rotation_matrices
  // here we use [x y z] = R * [1 0 0]
  var q0 = orientation.w;
  var q1 = orientation.x;
  var q2 = orientation.y;
  var q3 = orientation.z;
  // Canvas rotation is clock wise and in degrees
  return -Math.atan2(2 * (q0 * q3 + q1 * q2), 1 - 2 * (q2 * q2 + q3 * q3)) * 180.0 / Math.PI;
};

/**
 * @author Russell Toris - rctoris@wpi.edu
 */

/**
 * An image map is a PNG image scaled to fit to the dimensions of a OccupancyGrid.
 *
 * @constructor
 * @param options - object with following keys:
 *   * message - the occupancy grid map meta data message
 *   * image - the image URL to load
 */
ROS2D.ImageMap = function (options) {
  options = options || {};
  var message = options.message;
  var image = options.image;

  // save the metadata we need
  this.pose = new ROSLIB.Pose({
    position: message.origin.position,
    orientation: message.origin.orientation
  });

  // set the size
  this.width = message.width;
  this.height = message.height;

  // create the bitmap
  createjs.Bitmap.call(this, image);
  // change Y direction
  this.y = -this.height * message.resolution;

  // scale the image
  this.scaleX = message.resolution;
  this.scaleY = message.resolution;
  this.width *= this.scaleX;
  this.height *= this.scaleY;

  // set the pose
  this.x += this.pose.position.x;
  this.y -= this.pose.position.y;
};
ROS2D.ImageMap.prototype.__proto__ = createjs.Bitmap.prototype;

/**
 * @author Russell Toris - rctoris@wpi.edu
 */

/**
 * A image map is a PNG image scaled to fit to the dimensions of a OccupancyGrid.
 *
 * Emits the following events:
 *   * 'change' - there was an update or change in the map
 *
 * @constructor
 * @param options - object with following keys:
 *   * ros - the ROSLIB.Ros connection handle
 *   * topic (optional) - the map meta data topic to listen to
 *   * image - the image URL to load
 *   * rootObject (optional) - the root object to add this marker to
 */
ROS2D.ImageMapClient = function (options) {
  var that = this;
  options = options || {};
  var ros = options.ros;
  var topic = options.topic || '/map_metadata';
  this.image = options.image;
  this.rootObject = options.rootObject || new createjs.Container();

  // create an empty shape to start with
  this.currentImage = new createjs.Shape();

  // subscribe to the topic
  var rosTopic = new ROSLIB.Topic({
    ros: ros,
    name: topic,
    messageType: 'nav_msgs/MapMetaData'
  });

  rosTopic.subscribe(function (message) {
    // we only need this once
    rosTopic.unsubscribe();

    // create the image
    that.currentImage = new ROS2D.ImageMap({
      message: message,
      image: that.image
    });
    that.rootObject.addChild(that.currentImage);
    // work-around for a bug in easeljs -- needs a second object to render correctly
    that.rootObject.addChild(new ROS2D.Grid({
      size: 1
    }));

    that.emit('change');
  });
};
ROS2D.ImageMapClient.prototype.__proto__ = EventEmitter2.prototype;

/**
 * @author Russell Toris - rctoris@wpi.edu
 */

/**
 * An OccupancyGrid can convert a ROS occupancy grid message into a createjs Bitmap object.
 *
 * @constructor
 * @param options - object with following keys:
 *   * message - the occupancy grid message
 */
ROS2D.OccupancyGrid = function (options) {
  options = options || {};
  var message = options.message;

  // internal drawing canvas
  var canvas = document.createElement('canvas');
  var context = canvas.getContext('2d');

  // save the metadata we need
  this.pose = new ROSLIB.Pose({
    position: message.info.origin.position,
    orientation: message.info.origin.orientation
  });

  // set the size
  this.width = message.info.width;
  this.height = message.info.height;
  canvas.width = this.width;
  canvas.height = this.height;

  var imageData = context.createImageData(this.width, this.height);
  for (var row = 0; row < this.height; row++) {
    for (var col = 0; col < this.width; col++) {
      // determine the index into the map data
      var mapI = col + ((this.height - row - 1) * this.width);
      // determine the value
      var data = message.data[mapI];
      var val;
      if (data === 100) {
        val = 0;
      } else if (data === 0) {
        val = 255;
      } else {
        val = 127;
      }

      // determine the index into the image data array
      var i = (col + (row * this.width)) * 4;
      // r
      imageData.data[i] = val;
      // g
      imageData.data[++i] = val;
      // b
      imageData.data[++i] = val;
      // a
      imageData.data[++i] = 255;
    }
  }
  context.putImageData(imageData, 0, 0);

  // create the bitmap
  createjs.Bitmap.call(this, canvas);
  // change Y direction
  this.y = -this.height * message.info.resolution;

  // scale the image
  this.scaleX = message.info.resolution;
  this.scaleY = message.info.resolution;
  this.width *= this.scaleX;
  this.height *= this.scaleY;

  // set the pose
  this.x += this.pose.position.x;
  this.y -= this.pose.position.y;
};
ROS2D.OccupancyGrid.prototype.__proto__ = createjs.Bitmap.prototype;

/**
 * @author Russell Toris - rctoris@wpi.edu
 */

/**
 * A map that listens to a given occupancy grid topic.
 *
 * Emits the following events:
 *   * 'change' - there was an update or change in the map
 *
 * @constructor
 * @param options - object with following keys:
 *   * ros - the ROSLIB.Ros connection handle
 *   * topic (optional) - the map topic to listen to
 *   * rootObject (optional) - the root object to add this marker to
 *   * continuous (optional) - if the map should be continuously loaded (e.g., for SLAM)
 */
ROS2D.OccupancyGridClient = function (options) {
  var that = this;
  options = options || {};
  var ros = options.ros;
  var topic = options.topic || '/map';
  this.continuous = options.continuous;
  this.rootObject = options.rootObject || new createjs.Container();

  // current grid that is displayed
  // create an empty shape to start with, so that the order remains correct.
  this.currentGrid = new createjs.Shape();
  this.rootObject.addChild(this.currentGrid);
  // work-around for a bug in easeljs -- needs a second object to render correctly
  this.rootObject.addChild(new ROS2D.Grid({
    size: 1
  }));

  // subscribe to the topic
  var rosTopic = new ROSLIB.Topic({
    ros: ros,
    name: topic,
    messageType: 'nav_msgs/OccupancyGrid',
    compression: 'png'
  });

  rosTopic.subscribe(function (message) {
    // check for an old map
    var index = null;
    if (that.currentGrid) {
      index = that.rootObject.getChildIndex(that.currentGrid);
      that.rootObject.removeChild(that.currentGrid);
    }

    that.currentGrid = new ROS2D.OccupancyGrid({
      message: message
    });
    if (index !== null) {
      that.rootObject.addChildAt(that.currentGrid, index);
    } else {
      that.rootObject.addChild(that.currentGrid);
    }

    that.emit('change');

    // check if we should unsubscribe
    if (!that.continuous) {
      rosTopic.unsubscribe();
    }
  });
};
ROS2D.OccupancyGridClient.prototype.__proto__ = EventEmitter2.prototype;

/**
 * @author Jihoon Lee- jihoonlee.in@gmail.com
 * @author Russell Toris - rctoris@wpi.edu
 */

/**
 * A static map that receives from map_server.
 *
 * Emits the following events:
 *   * 'change' - there was an update or change in the map
 *
 * @constructor
 * @param options - object with following keys:
 *   * ros - the ROSLIB.Ros connection handle
 *   * service (optional) - the map topic to listen to, like '/static_map'
 *   * rootObject (optional) - the root object to add this marker to
 */
ROS2D.OccupancyGridSrvClient = function (options) {
  var that = this;
  options = options || {};
  var ros = options.ros;
  var service = options.service || '/static_map';
  this.rootObject = options.rootObject || new createjs.Container();

  // current grid that is displayed
  this.currentGrid = null;

  // Setting up to the service
  var rosService = new ROSLIB.Service({
    ros: ros,
    name: service,
    serviceType: 'nav_msgs/GetMap',
    compression: 'png'
  });

  rosService.callService(new ROSLIB.ServiceRequest(), function (response) {
    // check for an old map
    if (that.currentGrid) {
      that.rootObject.removeChild(that.currentGrid);
    }

    that.currentGrid = new ROS2D.OccupancyGrid({
      message: response.map
    });
    that.rootObject.addChild(that.currentGrid);

    that.emit('change', that.currentGrid);
  });
};
ROS2D.OccupancyGridSrvClient.prototype.__proto__ = EventEmitter2.prototype;

/**
 * @author Bart van Vliet - bart@dobots.nl
 */

/**
 * An arrow with line and triangular head, based on the navigation arrow.
 * Aims to the left at 0 rotation, as would be expected.
 *
 * @constructor
 * @param options - object with following keys:
 *   * size (optional) - the size of the marker
 *   * strokeSize (optional) - the size of the outline
 *   * strokeColor (optional) - the createjs color for the stroke
 *   * fillColor (optional) - the createjs color for the fill
 *   * pulse (optional) - if the marker should "pulse" over time
 */
ROS2D.ArrowShape = function (options) {
  var that = this;
  options = options || {};
  var size = options.size || 10;
  var strokeSize = options.strokeSize || 3;
  var strokeColor = options.strokeColor || createjs.Graphics.getRGB(0, 0, 0);
  var fillColor = options.fillColor || createjs.Graphics.getRGB(255, 0, 0);
  var pulse = options.pulse;

  // draw the arrow
  var graphics = new createjs.Graphics();

  var headLen = size / 3.0;
  var headWidth = headLen * 2.0 / 3.0;

  graphics.setStrokeStyle(strokeSize);
  graphics.beginStroke(strokeColor);
  graphics.moveTo(0, 0);
  graphics.lineTo(size - headLen, 0);

  graphics.beginFill(fillColor);
  graphics.moveTo(size, 0);
  graphics.lineTo(size - headLen, headWidth / 2.0);
  graphics.lineTo(size - headLen, -headWidth / 2.0);
  graphics.closePath();
  graphics.endFill();
  graphics.endStroke();

  // create the shape
  createjs.Shape.call(this, graphics);

  // check if we are pulsing
  if (pulse) {
    // have the model "pulse"
    var growCount = 0;
    var growing = true;
    createjs.Ticker.addEventListener('tick', function () {
      if (growing) {
        that.scaleX *= 1.035;
        that.scaleY *= 1.035;
        growing = (++growCount < 10);
      } else {
        that.scaleX /= 1.035;
        that.scaleY /= 1.035;
        growing = (--growCount < 0);
      }
    });
  }
};
ROS2D.ArrowShape.prototype.__proto__ = createjs.Shape.prototype;

/**
 * @author Raffaello Bonghi - raffaello.bonghi@officinerobotiche.it
 */

/**
 * A Grid object draw in map.
 *
 * @constructor
 * @param options - object with following keys:
 *  * size (optional) - the size of the grid
 *  * cellSize (optional) - the cell size of map
 *  * lineWidth (optional) - the width of the lines in the grid
 */
ROS2D.Grid = function (options) {
  var that = this;
  options = options || {};
  var size = options.size || 10;
  var cellSize = options.cellSize || 0.1;
  var lineWidth = options.lineWidth || 0.001;
  // draw the arrow
  var graphics = new createjs.Graphics();
  // line width
  graphics.setStrokeStyle(lineWidth * 5);
  graphics.beginStroke(createjs.Graphics.getRGB(0, 0, 0));
  graphics.beginFill(createjs.Graphics.getRGB(255, 0, 0));
  graphics.moveTo(-size * cellSize, 0);
  graphics.lineTo(size * cellSize, 0);
  graphics.moveTo(0, -size * cellSize);
  graphics.lineTo(0, size * cellSize);
  graphics.endFill();
  graphics.endStroke();

  graphics.setStrokeStyle(lineWidth);
  graphics.beginStroke(createjs.Graphics.getRGB(0, 0, 0));
  graphics.beginFill(createjs.Graphics.getRGB(255, 0, 0));
  for (var i = -size; i <= size; i++) {
    graphics.moveTo(-size * cellSize, i * cellSize);
    graphics.lineTo(size * cellSize, i * cellSize);
    graphics.moveTo(i * cellSize, -size * cellSize);
    graphics.lineTo(i * cellSize, size * cellSize);
  }
  graphics.endFill();
  graphics.endStroke();
  // create the shape
  createjs.Shape.call(this, graphics);

};
ROS2D.Grid.prototype.__proto__ = createjs.Shape.prototype;

/**
 * @author Russell Toris - rctoris@wpi.edu
 */

/**
 * A navigation arrow is a directed triangle that can be used to display orientation.
 *
 * @constructor
 * @param options - object with following keys:
 *   * size (optional) - the size of the marker
 *   * strokeSize (optional) - the size of the outline
 *   * strokeColor (optional) - the createjs color for the stroke
 *   * fillColor (optional) - the createjs color for the fill
 *   * pulse (optional) - if the marker should "pulse" over time
 */
ROS2D.NavigationArrow = function (options) {
  var that = this;
  options = options || {};
  var size = options.size || 10;
  var strokeSize = options.strokeSize || 3;
  var strokeColor = options.strokeColor || createjs.Graphics.getRGB(0, 0, 0);
  var fillColor = options.fillColor || createjs.Graphics.getRGB(255, 0, 0);
  var pulse = options.pulse;

  // draw the arrow
  var graphics = new createjs.Graphics();
  // line width
  graphics.setStrokeStyle(strokeSize);
  graphics.moveTo(-size / 2.0, -size / 2.0);
  graphics.beginStroke(strokeColor);
  graphics.beginFill(fillColor);
  graphics.lineTo(size, 0);
  graphics.lineTo(-size / 2.0, size / 2.0);
  graphics.closePath();
  graphics.endFill();
  graphics.endStroke();

  // create the shape
  createjs.Shape.call(this, graphics);

  // check if we are pulsing
  if (pulse) {
    // have the model "pulse"
    var growCount = 0;
    var growing = true;
    createjs.Ticker.addEventListener('tick', function () {
      if (growing) {
        that.scaleX *= 1.035;
        that.scaleY *= 1.035;
        growing = (++growCount < 10);
      } else {
        that.scaleX /= 1.035;
        that.scaleY /= 1.035;
        growing = (--growCount < 0);
      }
    });
  }
};
ROS2D.NavigationArrow.prototype.__proto__ = createjs.Shape.prototype;

/**
 * @author Inigo Gonzalez - ingonza85@gmail.com
 */

/**
 * A navigation image that can be used to display orientation.
 *
 * @constructor
 * @param options - object with following keys:
 *   * size (optional) - the size of the marker
 *   * image - the image to use as a marker
 *   * pulse (optional) - if the marker should "pulse" over time
 */
ROS2D.NavigationImage = function (options) {
  var that = this;
  options = options || {};
  var size = options.size || 10;
  var image_url = options.image;
  var pulse = options.pulse;
  var alpha = options.alpha || 1;

  var originals = {};

  var paintImage = function () {
    createjs.Bitmap.call(that, image);
    var scale = calculateScale(size);
    that.alpha = alpha;
    that.scaleX = scale;
    that.scaleY = scale;
    that.regY = that.image.height / 2;
    that.regX = that.image.width / 2;
    originals['rotation'] = that.rotation;
    Object.defineProperty(that, 'rotation', {
      get: function () {
        return originals['rotation'] + 90;
      },
      set: function (value) {
        originals['rotation'] = value;
      }
    });
    if (pulse) {
      // have the model "pulse"
      var growCount = 0;
      var growing = true;
      var SCALE_SIZE = 1.020;
      createjs.Ticker.addEventListener('tick', function () {
        if (growing) {
          that.scaleX *= SCALE_SIZE;
          that.scaleY *= SCALE_SIZE;
          growing = (++growCount < 10);
        } else {
          that.scaleX /= SCALE_SIZE;
          that.scaleY /= SCALE_SIZE;
          growing = (--growCount < 0);
        }
      });
    }
  };

  var calculateScale = function (_size) {
    return _size / image.width;
  };

  var image = new Image();
  image.onload = paintImage;
  image.src = image_url;

};

ROS2D.NavigationImage.prototype.__proto__ = createjs.Bitmap.prototype;

/**
 * @author Bart van Vliet - bart@dobots.nl
 */

/**
 * A shape to draw a nav_msgs/Path msg
 *
 * @constructor
 * @param options - object with following keys:
 *   * path (optional) - the initial path to draw
 *   * strokeSize (optional) - the size of the outline
 *   * strokeColor (optional) - the createjs color for the stroke
 */
ROS2D.PathShape = function (options) {
  options = options || {};
  var path = options.path;
  this.strokeSize = options.strokeSize || 3;
  this.strokeColor = options.strokeColor || createjs.Graphics.getRGB(0, 0, 0);

  // draw the line
  this.graphics = new createjs.Graphics();

  if (path !== null && typeof path !== 'undefined') {
    this.graphics.setStrokeStyle(this.strokeSize);
    this.graphics.beginStroke(this.strokeColor);
    this.graphics.moveTo(path.poses[0].pose.position.x / this.scaleX, path.poses[0].pose.position.y / -this.scaleY);
    for (var i = 1; i < path.poses.length; ++i) {
      this.graphics.lineTo(path.poses[i].pose.position.x / this.scaleX, path.poses[i].pose.position.y / -this.scaleY);
    }
    this.graphics.endStroke();
  }

  // create the shape
  createjs.Shape.call(this, this.graphics);
};

/**
 * Set the path to draw
 *
 * @param path of type nav_msgs/Path
 */
ROS2D.PathShape.prototype.setPath = function (path) {
  this.graphics.clear();
  if (path !== null && typeof path !== 'undefined') {
    this.graphics.setStrokeStyle(this.strokeSize);
    this.graphics.beginStroke(this.strokeColor);
    this.graphics.moveTo(path.poses[0].pose.position.x / this.scaleX, path.poses[0].pose.position.y / -this.scaleY);
    for (var i = 1; i < path.poses.length; ++i) {
      this.graphics.lineTo(path.poses[i].pose.position.x / this.scaleX, path.poses[i].pose.position.y / -this.scaleY);
    }
    this.graphics.endStroke();
  }
};

ROS2D.PathShape.prototype.__proto__ = createjs.Shape.prototype;

/**
 * @author Bart van Vliet - bart@dobots.nl
 */

/**
 * A polygon that can be edited by an end user
 *
 * @constructor
 * @param options - object with following keys:
 *   * pose (optional) - the first pose of the trace
 *   * lineSize (optional) - the width of the lines
 *   * lineColor (optional) - the createjs color of the lines
 *   * pointSize (optional) - the size of the points
 *   * pointColor (optional) - the createjs color of the points
 *   * fillColor (optional) - the createjs color to fill the polygon
 *   * lineCallBack (optional) - callback function for mouse interaction with a line
 *   * pointCallBack (optional) - callback function for mouse interaction with a point
 */
ROS2D.PolygonMarker = function (options) {
  //	var that = this;
  options = options || {};
  this.lineSize = options.lineSize || 3;
  this.lineColor = options.lineColor || createjs.Graphics.getRGB(0, 0, 255, 0.66);
  this.pointSize = options.pointSize || 10;
  this.pointColor = options.pointColor || createjs.Graphics.getRGB(255, 0, 0, 0.66);
  this.fillColor = options.pointColor || createjs.Graphics.getRGB(0, 255, 0, 0.33);
  this.lineCallBack = options.lineCallBack;
  this.pointCallBack = options.pointCallBack;

  // Array of point shapes
  //	this.points = [];
  this.pointContainer = new createjs.Container();

  // Array of line shapes
  //	this.lines = [];
  this.lineContainer = new createjs.Container();

  this.fillShape = new createjs.Shape();

  // Container with all the lines and points
  createjs.Container.call(this);

  this.addChild(this.fillShape);
  //this.addChild(this.lineContainer);
  this.addChild(this.pointContainer);
};

/**
 * Internal use only
 */
ROS2D.PolygonMarker.prototype.createLineShape = function (startPoint, endPoint) {
  var line = new createjs.Shape();
  //	line.graphics.setStrokeStyle(this.strokeSize);
  //	line.graphics.beginStroke(this.strokeColor);
  //	line.graphics.moveTo(startPoint.x, startPoint.y);
  //	line.graphics.lineTo(endPoint.x, endPoint.y);
  this.editLineShape(line, startPoint, endPoint);

  var that = this;
  line.addEventListener('mousedown', function (event) {
    if (that.lineCallBack !== null && typeof that.lineCallBack !== 'undefined') {
      that.lineCallBack('mousedown', event, that.lineContainer.getChildIndex(event.target));
    }
  });

  return line;
};

/**
 * Internal use only
 */
ROS2D.PolygonMarker.prototype.editLineShape = function (line, startPoint, endPoint) {
  line.graphics.clear();
  line.graphics.setStrokeStyle(this.lineSize);
  line.graphics.beginStroke(this.lineColor);
  line.graphics.moveTo(startPoint.x, startPoint.y);
  line.graphics.lineTo(endPoint.x, endPoint.y);
};

/**
 * Internal use only
 */
ROS2D.PolygonMarker.prototype.createPointShape = function (pos) {
  var point = new createjs.Shape();
  point.graphics.beginFill(this.pointColor);
  point.graphics.drawCircle(0, 0, this.pointSize);
  point.x = pos.x;
  point.y = -pos.y;

  var that = this;
  point.addEventListener('mousedown', function (event) {
    if (that.pointCallBack !== null && typeof that.pointCallBack !== 'undefined') {
      that.pointCallBack('mousedown', event, that.pointContainer.getChildIndex(event.target));
    }
  });

  return point;
};

/**
 * Adds a point to the polygon
 *
 * @param position of type ROSLIB.Vector3
 */
ROS2D.PolygonMarker.prototype.addPoint = function (pos) {
  var point = this.createPointShape(pos);
  this.pointContainer.addChild(point);
  var numPoints = this.pointContainer.getNumChildren();

  // 0 points -> 1 point, 0 lines
  // 1 point  -> 2 points, lines: add line between previous and new point, add line between new point and first point
  // 2 points -> 3 points, 3 lines: change last line, add line between new point and first point
  // 3 points -> 4 points, 4 lines: change last line, add line between new point and first point
  // etc

  if (numPoints < 2) {
    // Now 1 point
  } else if (numPoints < 3) {
    // Now 2 points: add line between previous and new point
    //var line = this.createLineShape(this.pointContainer.getChildAt(numPoints-2), point);
    //this.lineContainer.addChild(line);
  }
  if (numPoints > 2) {
    // Now 3 or more points: change last line
    //this.editLineShape(this.lineContainer.getChildAt(numPoints-2), this.pointContainer.getChildAt(numPoints-2), point);
  }
  if (numPoints > 1) {
    // Now 2 or more points: add line between new point and first point
    //var lineEnd = this.createLineShape(point, this.pointContainer.getChildAt(0));
    //this.lineContainer.addChild(lineEnd);
  }

  //this.drawFill();
};

/**
 * Removes a point from the polygon
 *
 * @param obj either an index (integer) or a point shape of the polygon
 */
ROS2D.PolygonMarker.prototype.remPoint = function (obj) {
  var index;
  //	var point;
  if (obj instanceof createjs.Shape) {
    index = this.pointContainer.getChildIndex(obj);
    //		point = obj;
  } else {
    index = obj;
    //		point = this.pointContainer.getChildAt(index);
  }

  // 0 points -> 0 points, 0 lines
  // 1 point  -> 0 points, 0 lines
  // 2 points -> 1 point,  0 lines: remove all lines
  // 3 points -> 2 points, 2 lines: change line before point to remove, remove line after point to remove
  // 4 points -> 3 points, 3 lines: change line before point to remove, remove line after point to remove
  // etc

  var numPoints = this.pointContainer.getNumChildren();

  if (numPoints < 2) {

  } else if (numPoints < 3) {
    // 2 points: remove all lines
    this.lineContainer.removeAllChildren();
  } else {
    // 3 or more points: change line before point to remove, remove line after point to remove
    this.editLineShape(
      this.lineContainer.getChildAt((index - 1 + numPoints) % numPoints),
      this.pointContainer.getChildAt((index - 1 + numPoints) % numPoints),
      this.pointContainer.getChildAt((index + 1) % numPoints)
    );
    this.lineContainer.removeChildAt(index);
  }
  this.pointContainer.removeChildAt(index);
  //	this.points.splice(index, 1);

  //this.drawFill();
};

/**
 * Moves a point of the polygon
 *
 * @param obj either an index (integer) or a point shape of the polygon
 * @param position of type ROSLIB.Vector3
 */
ROS2D.PolygonMarker.prototype.movePoint = function (obj, newPos) {
  var index;
  var point;
  if (obj instanceof createjs.Shape) {
    index = this.pointContainer.getChildIndex(obj);
    point = obj;
  } else {
    index = obj;
    point = this.pointContainer.getChildAt(index);
  }
  point.x = newPos.x;
  point.y = -newPos.y;

  var numPoints = this.pointContainer.getNumChildren();
  if (numPoints > 1) {
    // line before moved point
    var line1 = this.lineContainer.getChildAt((index - 1 + numPoints) % numPoints);
    this.editLineShape(line1, this.pointContainer.getChildAt((index - 1 + numPoints) % numPoints), point);

    // line after moved point
    var line2 = this.lineContainer.getChildAt(index);
    this.editLineShape(line2, point, this.pointContainer.getChildAt((index + 1) % numPoints));
  }

  //this.drawFill();
};

/**
 * Splits a line of the polygon: inserts a point at the center of the line
 *
 * @param obj either an index (integer) or a line shape of the polygon
 */
ROS2D.PolygonMarker.prototype.splitLine = function (obj) {
  var index;
  var line;
  if (obj instanceof createjs.Shape) {
    index = this.lineContainer.getChildIndex(obj);
    line = obj;
  } else {
    index = obj;
    line = this.lineContainer.getChildAt(index);
  }
  var numPoints = this.pointContainer.getNumChildren();
  var xs = this.pointContainer.getChildAt(index).x;
  var ys = this.pointContainer.getChildAt(index).y;
  var xe = this.pointContainer.getChildAt((index + 1) % numPoints).x;
  var ye = this.pointContainer.getChildAt((index + 1) % numPoints).y;
  var xh = (xs + xe) / 2.0;
  var yh = (ys + ye) / 2.0;
  var pos = new ROSLIB.Vector3({
    x: xh,
    y: -yh
  });

  // Add a point in the center of the line to split
  var point = this.createPointShape(pos);
  this.pointContainer.addChildAt(point, index + 1);
  ++numPoints;

  // Add a line between the new point and the end of the line to split
  var lineNew = this.createLineShape(point, this.pointContainer.getChildAt((index + 2) % numPoints));
  this.lineContainer.addChildAt(lineNew, index + 1);

  // Set the endpoint of the line to split to the new point
  this.editLineShape(line, this.pointContainer.getChildAt(index), point);

  //this.drawFill();
};

/**
 * Internal use only
 */
ROS2D.PolygonMarker.prototype.drawFill = function () {
  var numPoints = this.pointContainer.getNumChildren();
  if (numPoints > 2) {
    var g = this.fillShape.graphics;
    g.clear();
    g.setStrokeStyle(0);
    g.moveTo(this.pointContainer.getChildAt(0).x, this.pointContainer.getChildAt(0).y);
    g.beginStroke();
    g.beginFill(this.fillColor);
    for (var i = 1; i < numPoints; ++i) {
      g.lineTo(this.pointContainer.getChildAt(i).x, this.pointContainer.getChildAt(i).y);
    }
    g.closePath();
    g.endFill();
    g.endStroke();
  } else {
    this.fillShape.graphics.clear();
  }
};


ROS2D.PolygonMarker.prototype.__proto__ = createjs.Container.prototype;

/**
 * @author Bart van Vliet - bart@dobots.nl
 */

/**
 * A trace of poses, handy to see where a robot has been
 *
 * @constructor
 * @param options - object with following keys:
 *   * pose (optional) - the first pose of the trace
 *   * strokeSize (optional) - the size of the outline
 *   * strokeColor (optional) - the createjs color for the stroke
 *   * maxPoses (optional) - the maximum number of poses to keep, 0 for infinite
 *   * minDist (optional) - the minimal distance between poses to use the pose for drawing (default 0.05)
 */
ROS2D.TraceShape = function (options) {
  //	var that = this;
  options = options || {};
  var pose = options.pose;
  this.strokeSize = options.strokeSize || 3;
  this.strokeColor = options.strokeColor || createjs.Graphics.getRGB(0, 0, 0);
  this.maxPoses = options.maxPoses || 100;
  this.minDist = options.minDist || 0.05;

  // Store minDist as the square of it
  this.minDist = this.minDist * this.minDist;

  // Array of the poses
  // TODO: do we need this?
  this.poses = [];

  // Create the graphics
  this.graphics = new createjs.Graphics();
  this.graphics.setStrokeStyle(this.strokeSize);
  this.graphics.beginStroke(this.strokeColor);

  // Add first pose if given
  if (pose !== null && typeof pose !== 'undefined') {
    this.poses.push(pose);
  }

  // Create the shape
  createjs.Shape.call(this, this.graphics);
};

/**
 * Adds a pose to the trace and updates the graphics
 *
 * @param pose of type ROSLIB.Pose
 */
ROS2D.TraceShape.prototype.addPose = function (pose) {
  var last = this.poses.length - 1;
  if (last < 0) {
    this.poses.push(pose);
    this.graphics.moveTo(pose.position.x / this.scaleX, pose.position.y / -this.scaleY);
  } else {
    var prevX = this.poses[last].position.x;
    var prevY = this.poses[last].position.y;
    var dx = (pose.position.x - prevX);
    var dy = (pose.position.y - prevY);
    if (dx * dx + dy * dy > this.minDist) {
      this.graphics.lineTo(pose.position.x / this.scaleX, pose.position.y / -this.scaleY);
      this.poses.push(pose);
    }
  }
  if (this.maxPoses > 0 && this.maxPoses < this.poses.length) {
    this.popFront();
  }
};

/**
 * Removes front pose and updates the graphics
 */
ROS2D.TraceShape.prototype.popFront = function () {
  if (this.poses.length > 0) {
    this.poses.shift();
    // TODO: shift drawing instructions rather than doing it all over
    this.graphics.clear();
    this.graphics.setStrokeStyle(this.strokeSize);
    this.graphics.beginStroke(this.strokeColor);
    this.graphics.lineTo(this.poses[0].position.x / this.scaleX, this.poses[0].position.y / -this.scaleY);
    for (var i = 1; i < this.poses.length; ++i) {
      this.graphics.lineTo(this.poses[i].position.x / this.scaleX, this.poses[i].position.y / -this.scaleY);
    }
  }
};

ROS2D.TraceShape.prototype.__proto__ = createjs.Shape.prototype;

/**
 * @author Bart van Vliet - bart@dobots.nl
 */

/**
 * Adds panning to a view
 *
 * @constructor
 * @param options - object with following keys:
 *   * rootObject (optional) - the root object to apply panning to
 */
ROS2D.PanView = function (options) {
  options = options || {};
  this.rootObject = options.rootObject;

  // get a handle to the stage
  if (this.rootObject instanceof createjs.Stage) {
    this.stage = this.rootObject;
  } else {
    this.stage = this.rootObject.getStage();
  }

  this.startPos = new ROSLIB.Vector3();
};


ROS2D.PanView.prototype.startPan = function (startX, startY) {
  this.startPos.x = startX;
  this.startPos.y = startY;
};

ROS2D.PanView.prototype.pan = function (curX, curY) {
  this.stage.x += curX - this.startPos.x;
  this.startPos.x = curX;
  this.stage.y += curY - this.startPos.y;
  this.startPos.y = curY;
};

/**
 * @author Russell Toris - rctoris@wpi.edu
 */

/**
 * A Viewer can be used to render an interactive 2D scene to a HTML5 canvas.
 *
 * @constructor
 * @param options - object with following keys:
 *   * divID - the ID of the div to place the viewer in
 *   * width - the initial width, in pixels, of the canvas
 *   * height - the initial height, in pixels, of the canvas
 *   * background (optional) - the color to render the background, like '#efefef'
 */
ROS2D.Viewer = function (options) {
  var that = this;
  options = options || {};
  var divID = options.divID;
  this.width = options.width;
  this.height = options.height;
  var background = options.background || '#111111';

  // create the canvas to render to
  var canvas = document.createElement('canvas');
  canvas.width = this.width;
  canvas.height = this.height;
  canvas.style.background = background;
  document.getElementById(divID).appendChild(canvas);
  // create the easel to use
  this.scene = new createjs.Stage(canvas);

  // change Y axis center
  this.scene.y = this.height;

  // add the renderer to the page
  document.getElementById(divID).appendChild(canvas);

  // update at 30fps
  createjs.Ticker.setFPS(30);
  createjs.Ticker.addEventListener('tick', this.scene);
};

/**
 * Add the given createjs object to the global scene in the viewer.
 *
 * @param object - the object to add
 */
ROS2D.Viewer.prototype.addObject = function (object) {
  this.scene.addChild(object);
};

/**
 * Scale the scene to fit the given width and height into the current canvas.
 *
 * @param width - the width to scale to in meters
 * @param height - the height to scale to in meters
 */
ROS2D.Viewer.prototype.scaleToDimensions = function (width, height) {
  // restore to values before shifting, if ocurred
  this.scene.x = typeof this.scene.x_prev_shit !== 'undefined' ? this.scene.x_prev_shit : this.scene.x;
  this.scene.y = typeof this.scene.y_prev_shift !== 'undefined' ? this.scene.y_prev_shift : this.scene.y;

  // save scene scaling
  this.scene.scaleX = this.width / width;
  this.scene.scaleY = this.height / height;
};

/**
 * Shift the main view of the canvas by the given amount. This is based on the
 * ROS coordinate system. That is, Y is opposite that of a traditional canvas.
 *
 * @param x - the amount to shift by in the x direction in meters
 * @param y - the amount to shift by in the y direction in meters
 */
ROS2D.Viewer.prototype.shift = function (x, y) {
  // save current offset
  this.scene.x_prev_shit = this.scene.x;
  this.scene.y_prev_shift = this.scene.y;

  // shift scene by scaling the desired offset
  this.scene.x -= (x * this.scene.scaleX);
  this.scene.y += (y * this.scene.scaleY);
};

/**
 * @author Bart van Vliet - bart@dobots.nl
 */

/**
 * Adds zooming to a view
 *
 * @constructor
 * @param options - object with following keys:
 *   * rootObject (optional) - the root object to apply zoom to
 *   * minScale (optional) - minimum scale to set to preserve precision
 */
ROS2D.ZoomView = function (options) {
  options = options || {};
  this.rootObject = options.rootObject;
  this.minScale = options.minScale || 0.001;

  // get a handle to the stage
  if (this.rootObject instanceof createjs.Stage) {
    this.stage = this.rootObject;
  } else {
    this.stage = this.rootObject.getStage();
  }

  this.center = new ROSLIB.Vector3();
  this.startShift = new ROSLIB.Vector3();
  this.startScale = new ROSLIB.Vector3();
};


ROS2D.ZoomView.prototype.startZoom = function (centerX, centerY) {
  this.center.x = centerX;
  this.center.y = centerY;
  this.startShift.x = this.stage.x;
  this.startShift.y = this.stage.y;
  this.startScale.x = this.stage.scaleX;
  this.startScale.y = this.stage.scaleY;
};

ROS2D.ZoomView.prototype.zoom = function (zoom) {
  // Make sure scale doesn't become too small
  if (this.startScale.x * zoom < this.minScale) {
    zoom = this.minScale / this.startScale.x;
  }
  if (this.startScale.y * zoom < this.minScale) {
    zoom = this.minScale / this.startScale.y;
  }

  this.stage.scaleX = this.startScale.x * zoom;
  this.stage.scaleY = this.startScale.y * zoom;

  this.stage.x = this.startShift.x - (this.center.x - this.startShift.x) * (this.stage.scaleX / this.startScale.x - 1);
  this.stage.y = this.startShift.y - (this.center.y - this.startShift.y) * (this.stage.scaleY / this.startScale.y - 1);
};